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ABSTRACT 


There are two goals for autonomous vehicle navigation planning: shortest 
path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearance between the vehicle and obstacles. Because a Voronoi boundary is the 
set of points locally maximizing the clearance from obstacles, safety is maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 
autonomous vehicles. 

We use the derivative of curvature k of the vehicle motion (dk/ds) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
11 at the Naval Postgraduate School used a path tracking method. Before the 
mission began the vehicle was given a track to follow; motion planning consisted 
of calculating the point on the track closest to the vehicle and calculating dx/ds 
then steering the vehicle to get onto the track. 

We propose a method of planning safe motions of the vehicle to calculate 
optimal dx/ds at each point directly from the information of the world without 
calculating the track to follow. This safe navigation algorithm is fundamentally 
different from path tracking using a path specification. Additionally, motion 
planning is simpler and faster than the path tracking method. 

The effectiveness of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the autonomous mobile 


robot Yamabico 11 at the Naval Postgraduate School. 
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THESIS DISCLAIMER 


The reader is cautioned that computer programs developed in this research 
may not have been exercised for all cases of interest. While every effort has been 
made, within the time available, to ensure that the programs are free of 
computational and logic errors, they cannot be considered validated. Any 
application of these programs without additional verification is at the risk of the 


user. 
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EXECUTIVE SUMMARY 


There are two goals for autonomous vehicle navigation planning: shortest 
path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearances between the vehicle and obstacles. Because a Voronoi boundary ts the 
set of points locally maximizing the clearance from obstacles, safety is maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 
autonomous vehicles. 

We use the derivative of curvature x of the vehicle motion (dx/ds) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
11 at the Naval Postgraduate School used a path tracking method. Before the 
mission began the vehicle was given a track to follow; motion planning consisted 
of calculating the point on the track closest to the vehicle and calculating dx/ds 
then steering the vehicle to get onto the track. The safest path through the world 
navigated by the vehicle is the set of points locally maximizing the clearance from 
obstacles. This path is represented by the Voronoi diagram. To achieve the path 
tracking method it is necessary to calculate the Voronoi boundary which consists 
of line segments and parabolic arcs. If the world is large, it is complicated and 
inefficient to calculate every Voronoi boundary of this world. It is better to 
calculate optimal dx/ds at each point directly from the information of the world 
without calculating the track to follow. 

When the objects are two points, two lines, or one point and one line, we can 


safely navigate the vehicle to achieve equal clearances from these objects. The 
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motion of the vehicle is optimized at each point directly from the information of 
the obstacles near the vehicle. After calculating dx/ds, the vehicle follows the 
Voronoi boundaries defined by the environment. 

Unlike the path tracking method, this method can be applied to avoid moving 
objects since we calculate the optimal motion of the vehicle at each point directly 
from the information of the world. Additionally, motion control is simpler and 
faster than in the path tracking method. 

The effectiveness of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the autonomous mobile 
robot Yamabico 11 at the Naval Postgraduate School. It has precise knowledge of 
its location in a given environment using its sonar system. The robot is 
programmed by the high level mobile robot language called MML (Model-based 
Mobile robot Language) written in the C language. 
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I. INTRODUCTION 


A. BACKGROUND 

There are two goals for planning autonomous vehicle navigation planning: 
shortest path and safe path. These goals are often in conflict; path safety is more 
important. Safety of autonomous vehicle navigation is determined by the 
clearance between the ve..icle and obstacles. Because a Voronoi boundary is the 
set of points locally maximizing the clearance from obstacles, safety 1s maximized 
on it. Therefore, Voronoi Diagrams are suitable for motion planning of 


autonomous vehicles. 


B. OVERVIEW 

We use the derivative of curvature x of the vehicle motion (dxk/as) as the 
only control variable for the vehicle, where s is the length along the vehicle 
trajectory. Previous motion planning of the autonomous mobile robot Yamabico- 
11 at the Naval Postgraduate School used a path tracking method [Ref. 2]. Before 
the mission began the vehicle was given a track to follow; motion planning 
consisted of calculating the point on the track closest to the vehicle and 
calculating dx/ds then steering the vehicle to get onto the track. The safest path 
through the world navigated by the vehicle is the set of points locally maximizing 
the clearance from obstacles. This path is represented by the Voronoi diagram. To 
achieve the path tracking method it is necessary to calculate the Voronoi 
boundary which consists of line segments and parabolic arcs. If the world is large, 


it is complicated and inefficient to calculate every Voronoi boundary of this 


world. It is better to calculate optimal dx/ds at each point directly from the 
information of the world without calculating the track to follow. 

This safe navigation algorithm is fundamentally different from path tracking 
using a path specification. Additionally, motion planning is simpler and faster than 
in the path tracking method. 

The effectiveness of this steering function for vehicle motion control 1s 
demonstrated by algorithmic simulation and by use on the autonomous mobile 
robot Yamabico 1] at the Naval Postgraduate School. It has precise knowledge of 
its location in a given environment using its sonar system. The robot is 
programmed by the high level mobile robot language called MML (Model-based 
Mobile robot Language) written in the C language [Ref. 3]. 


Il. PROBLEM STATEMENT 


The problems addressed are as follows: 

1. How to navigate the robot safely to achieve the same clearance from two 
points. 

2. How to navigate the robot safely to achieve the same clearance from two 
lines. 

3. How to navigate the robot safely to achieve the same clears. 2 from one 
point and one line. 

4. How to execute the safest path by a real robot. 

Safety is one of the important attributes for autonomous vehicle navigation 
planning. Safety is determined by the clearance between the vehicle and objects. 
Assume the vehicle is a point object and W(p) is the clearance of the point p. The 
clearance of a path represents its safety. If the clearance is small, the path is 
dangerous because it 1s close to the object and if it is larger, the path 1s safer. The 
clearance of a path IT is defined as: 

WI) = Min W(P) (2.1) 
Where IT is the path of the vehicle from start S to goal G. 

We want to find the path II, such that W(II,) 1s the maximum among all 

possible paths (Figure 2.1). To maximize the clearance W(II), we will take the 


Vorono1 boundary as the path of the vehicle. 





Ill. VORONOI DIA :-RAM 


A. DEFINITIONS 

Assume that there is an object o in a plane. It might be a point, a line, a line 
segment, a polygon, or other closed sets of points. If a world W has more than one 
object, a Voronoi region of an object o, in the world is defined as 

V(o,) =[PIV[i # j — {dist(p,o,) < dist(p,o,)}]] (3.1) 

Where p is a point which is not inside o, and diSst(p,o,) is the minimum 
distance between p and o,. 

The set of all the Voronoi regions is called the Voronoi diagram of a world. 
The boundaries of Voronoi regions are Voronoi boundaries. The Voronoi 
diagram of a geometric world typically consists of lines, rays, line segments, and 
parabolic arcs. 

B. VORONOI DIAGRAM OF TWO POINTS 
In the case that a world consists of two distinct points, p, and p,, its Voronoi 


boundary is the bisector of those two points which generate two Voronoi regions 


V(p,) and V(p,) (Figure 3.1). 


P, =(%,y;) 
 ) 
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Figure 3.1 : Voronoi Diagram of Two Points 


C. VORONOI DIAGRAM OF TWO LINES 
In the case that a world consists of two lines L, and L, which are not parallel 


to each other, their Voronoi boundaries consist of the bisectors of two lines which 
generate eight Voronoi regions V(L,), V(L,), V(L3), V(L,4), V(L,), V(L,), 
V(L,;) and V(L,,) (Figure 3.2). 
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Figure 3.2 ; Voronoi Diagram of Two Lines (Not Parallel) 


D. VORONOI DIAGRAM OF TWO PARALLEL LINES 

In the case that a world consists of two parallel lines L, and L,, their Voronoi 
boundary is one line which is parallel to L, and L, which generates four Voronoi 
regions V(L,), V(L,), V(L,,) and V(L,,), (Figure 3.3). 
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Figure 3.3 : Voronoi Diagram of Two Parallel Lines 





E. VORONOI DIAGRAM OF A LINE SEGMENT 

In the case that a world consists of one closed line segment p,p,, we treat it 
as a union of three objects : two end points p,, p, and an open line segment 
e = p,p,. A closed line segment includes both endpoints, but an open line segment 
does not. Therefore, its Voronoi boundaries consists of two lines which generate 


four Voronoi regions V(p,), V(p,), V(e,) and V(e,). (Figure 3.4) 


P, V(P,) 


Voronoi boundaries 





Figure 3.4 Voronoi Diagram of A Line Segment 


F. VORONOI DIAGRAM OF A POINT AND A LINE 


In the case that a world consists of a point p, and a line q,, its Voronoi 
boundary is a parabola which generates three Voronoi regions V(p,), V(e,), and 


V(e,). The point p, 1s the focus and the line gq, is the directrix of the parabola 


(Figure 3.5). 





Figure 3.5 ; Voronoi Diagram of A Point and A Line 


G. VORONOI DIAGRAM OF A POINT AND TWO LINES 

In the case that a world consists of a point p that is between two parallel 
lines L, and L,, their Voronoi boundaries are two parabolas (focus p, directrix L, 
and focus p, directrix L,) and the bisector of the line L, and L, which generate 
five Voronoi regions V(p), V(e,), V(e,), V(e,) and V(e,) (Figures 3.6 and 3.7). 
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Fig 3.6 : Voronoi Diagram of A Center Point and Two Lines 
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Figure 3.7 : Voronoi Diagram of A Point and Two Lines 


H. VORONOI DIAGRAM OF LINE AND A RAY 
Assume a ray is a kind of line which has only one end point p. In the case 


that a world consists of a line L, and a ray L,, which is orthogonal to the line L, 


their Voronoi boundaries are a line segment p,p,, and two bisectors of the line L, 
and the ray L,, and a parabola (focus p and directrix L,), which generates five 
Voronoi regions V(p), V(e,), V(e,), V(e;) and V(e,) (Figure 3.8). 
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Figure 3.8 ; Voronoi Diagram of Line and Ray 


L VORONOI DIAGRAM OF TWO LINES AND A LINE SEGMENT 

In the case that a world consists of two parallel lines L, L, and a closed line 
segment p,p,, which is parallel to lines L, and L,, their Voronoi boundaries are 
bisectors of the line Z, and L, the bisector of the line ZL, and the line segment 
P,p,, the bisector of the line L, and the line segment p,p,, two closed line 


segment p,p, and p.p, and parabolas (Figures 3.9 and 3.10). 
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Figure 3.10 : Voronoi Diagram of Two Lines and A Line Segment 


1] 


J. VORONOI DIAGRAM OF A NORMAL POLYGON 
In the case that a world has only one normal polygon, we treat it as a union 


of vertices (points) p, and open edges e,. There are Voronoi boundaries to the 


polygon shown in (Figure 3.11). 


V(e3) 


P3 


oo 


DP 


Ve) 





Figure 3.11 : Voronoi Diagram of A Norma! Polygon 


K. VORONOI DIAGRAM OF AN INVERTED POLYGON 


In the case that a world has only one inverted polygon, we also treat it as a 


union of vertices (points) p, and open edges e,. There are Voronoi boundaries in 


its interior (Figure 3.12). 
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Figure 3.12 : Voronoi Diagram of An Inverted Polygon 





IV. CURVE GENERATION 


A. CONFIGURATION 
To navigate a rigid body robot vehicle, the vehicle's state can be described by 
its current configuration, 

q = (p,8) (4.1) 
where p is the vehicle's current coordinate position (x,y), and @ is the vehicle's 
tangent orientation at that point. 

Let x be the derivative of tangent orientation with respect to the length 


along the trajectory s which is called curvature. 


d@(s) 


ms (4.2) 


K(s)= 


In this case, kK 1S not included in the configuration. However, « can be included 


in the configuration if « 1s required for the calculation. 


B. NEXT FUNCTION 
In order to compute the sequence of configurations, it is suffice to compute 


: ; dk 
the next configuration at each step As. Given As and (=), we can estimate the 


vehicle's next configuration at s+ As as follows. 
1. Short Circular Segment 
The short path segment between s and 5+ As 1s approximated by a circular 


curve segment (Figure 4.1). 
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X 


(0,r —r-cos(A@)) Fe 


\ : qi = nee as am a 
4, = ((e.¥4), 8) =((0,0),0) IM AP.r(d—cos(A9), 


Figure 4.1 : Short Circular Segment 


Assume configurations of both end points are g, and q,. 
do ae (X53. )s 6,) a ((0,0),0) 
q, = ((X,,y,) 6.) 


Let us assume A@ #0. The radius of the short circular segment is 


Where 
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(4.3) 
(4.4) 


(4.5) 


(4.6) 





The configuration of the end point g, is 
) ( rsin(A@) (sin(A@)/A@)As 
d=! \y, ) |=] (rd —cos(A@)) } |=] | ((1-cos(A@))/A@)As } | Gf A #0) 
6 A@é A@é 


Gems 


Using the Taylor expansion forms for the sin and cos functions 


sin(A@) _ A@-(A@)*/3!+ (AG) /S!+ _ (AQ) (Aé)* 


| -——— + ———_ -.... 
28 A@ 3! 5! 
1. s0s(A@) _ 1-(1-(A@)’/2!+ (A@)*/4!- (A)? /6!- --) 
A@ A@ 


-($-G0 OO" so 
2) 4! 6! 


Therefore, the configuration of the point gq, is 


"| (1—(A0)?/3!+ (A@)*/5!—---)As 
q =1\y,) |=] 0/2! - (Ae) /41 + (A6)4 /6! —---)A@As 
0, A@ 
In general, 

A@ «1 


Therefore equation (4.11) can be approximated as 
( ) (1-(A6)*/3!)As 
N= Vy) |=] d/2!—(Aey /4)A@As 
| 6, AG 
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(4.7) 


(4.8) 


(4.9) 


(4.10) 


(4.11) 


(4.12) 


(4.13) 


2. Global Position Calculation 
By using the composition function o [Ref. 1] which is a two dimensional 
coordinate transformation from the local coordinate system (x,,y,) to the global 


coordinate system (x,,y)), we can calculate the global position of the vehicle at 


s+As as follows. 


X, +x, cos 6, — y, sin 8, 
q(s+As)=q, Oo q =| ¥) +X, sin 8, + y, cos 4, (4.14) 
6, + 6, 


Where gq, and q, are given Equation (4.3) and Equation (4.13). 
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V. SAFE NAVIGATION USING A STEERING FUNCTION 


We use the derivative of curvature as the only control variable for the 


vehicle. 


= = f(p,9,K) (5.1) 


Therefore the vehicle's motion is controlled only through changing its curvature. 


Ax = (= as (5.2) 


We propose the steering function in the following form 


“ = —(aAx + bA@ + cAd) (2:3) 
or 

dk 

Pa ee (5.4) 


where a,b,c are positive constants and Ax, A@, Ad are variables. Each 
evaluation of Ax, A@, Ad differs in the situation of the obstacles (in the case that 
the obstacles are one point and one line, the obstacles are two lines, and the 


obstacles are two points). 


A. LINE TRACKING 
Consider a special case of the line tracking (Figure 5.1). Assume we want to 
track the X-axis of the global coordinate system, then we can define three 


positive constants a, b,c. 
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q = ((x, y), , kK) 





Figure 5.1 : Line Tracking 


On the X-axis, 
oi) (5.5) 
Therefore 
AK=K-K,,=K (5.6) 
A6=0-6,, =0 (5.7) 
as, (5.8) 


Let us now consider a short path segment (Figure 5.2). 





Figure 5.2 : Short Path Segment 
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The vehicle's tangent orientation @ is specified by 


ng 
Ax 
Then 
peti rane (as Ax > 0) 
Ax 
t\3 1\5 
ee) OnE 
3 5 


As 1s defined as 


As = y(Ax)’ + (Ay)’ 


Therefore 
2 2 Z 
As _ y(Ax) + (Ay) i+(22) = f14(y > (as Ax + 0) 
Ax Ax Ax 
From the definition 
es 
FR y y" 








= (tan y')= = 
des sd «14+? 


From Equation (5.12) and Equation (5.13) 


d Bae y 
28 8 antyy oe Aw 1+ (yy = ua 
=) _— = ; 
ds ds as 1+ ayy? 
dx 
Ther<‘ore 
dk 3 
dk _ io 07 
ds 
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dee - pra ee meas fh 1" 271 1\2\-3 
ds Voy y"'(+(y')’) yy 0 Oy) 
dx 


(5.9) 


(5.10) 


(3,05) 


(5.12) 


(5.13) 


(© 14) 


(5.15) 


Assume 


y?«l (5.16) 
y(y"yr« y" (5.17) 
Then from Equation (5.10) 
A@=y' (5.18) 
From Equation (5.14) 
Ak=y'' (5:13) 
From Equation (5.15) 
ue =" (5.20) 


Equation (5.4) becomes an ordinary differential equation: 

y'''+ay''+by'+cy =0 (5.21) 

(D> +aD’? +bD+c)y=0 Gee 
Since this is a third order linear homogeneous ordinary differential equation with 
constant coefficients, it must have at least one real root. If it has a non-negative 
root, it does not have a converging solution. If it has a complex conjugate root, 
the solution oscillates even if it decays. Since we want non-oscillatory decaying 
solutions, Equation (5.22) must have three negative roots of D. Also if we want a 
critical damping solution then Equation (5.22) must be specified to have a triple 
root, —k (where k > 0). 

D?+aD*+bD+c=(D+ky =D? +3kD?4+3KPD+k> = (5.23) 


Therefore if we choose 


a=3k (5.24) 
b = 3k? (5.25) 
C=k (5.26) 


Equation (5.22) becomes 


Le 


(D+k)y=0 (5.27) 
Thus, under this condition, there is only one degree of freedom in choosing the 


parameter k instead of three parameters a, b and c. We define 


ae (5.28) 


This size constant s, controls the distance for which the vehicle runs before it 


gets on track. A smaller size constant makes the transition distance smaller. Thus 


S, controls the sharpness of the trajectory. From Equations (5.3), (5.24), (5.25), 
(5.26) and (5.28), the revised steering function becomes 


cS {zac soy 2 J (5.29) 
ds So So. So 


Ax, AO, Ad are evaluated depending upon the environment. Let consider 
three situations for vehicle navigation: the objects are two points, the objects are 


two lines, the objects are one point and one line. 


B. TWO POINTS 

When we navigate the vehicle safely to make the same clearance from two 
points, its trajectory becomes a line which is a bisector of the two points. It is a 
Voronoi boundary of the two points. Let consider the case of the world consists 


of two points p, and p, (Figure 5.3). 
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Figure 5.3 : Safe Navigation of Two Points 


1. Evaluation of Ax 
When the vehicle's configuration is g =(p,@,x), 
AK=K (5.30) 
Since final value of x on the Voronoi boundary is zero. 
2. Evaluation of A@ 
Let Y(p,p,) denote the orientation from p to p, and (p,p,) denote 
the orientation from p to p,. Let @ be the difference between the orientation 


‘Y(p, p,) and the vehicle's orientation @; 8 is the difference between the vehicle's 
orientation 6 and ‘Y(p,p,) (Figure 5.4). 


DP = Ga) 


q = (p, 9, K) 





Y(p, P2) Py = (3,92) 


Figure 5.4 : Evaluation of A@ 
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a =¥(p, p,)- 6 (5.31) 


B = O—‘(p, p,) (5,32) 
When vehicle is on the Voronoi boundary, 
a (3,33) 


Let the desired orientation be @,. At the start point 


0, = normalizey{ PP I* TPP.) —P(P Po + 'P(D, Po) (5.34) 


Where normalizel is a function which normalizes its argument into a range of 


|-3.3] by addition of tnz if necessary and p, is the middle point between p, 


and p,. At the other point, 


6,= normalizey{ PP) TP. P.) - Sine + Oa 235) 
Where 6,,,, is the vehicle's previous desired orientation 6, at the point. Then the 


variable A@ is evaluated as 
A@=6-6, (5.36) 

3. Evaluation of Ad 
Let d, be the distance between p and p,, and d, be the distance from p 


to p,. 


d,=(x-x,) +(y-y,)’ (5.37) 
d, = (x-x,) +(y-y,) (5.38) 
The signed variable Ad is evaluated as follows (Figures 5.5 and 5.6). Note that 


Ad can be signed positive or negative, or equal zero. 


ZS 


P = (x,,y,) 


Y(D, P;) 


q =(p,@,k) 


(P,P) 





P, = (%5y>.) 


Figure 5.5 : Case Y(p,p,)—‘Y¥(p,p,) >0 


(PD, P,) 


q=(p,6@,k) 


(DP, P,) 





P, = (x,,),) 


Figure 5.6 : Case Y(p,p,)—‘Y¥(p,p,) <0 


Ad=d,-d, (if ‘¥(p,p,)—'¥(p,p,) > 0) (5.39) 
Ad=d,~-d, (if ¥(p,p,)—‘¥(p, p,) <0) (5.40) 
4. Simulation Results 

The use of this steering function for vehicle motion control is 
demonstrated by algorithmic simulation and by use on the existing robot 
Yamabico 11. The result is shown in Figures 5.7 and 5.8. Figure 5.7 shows the 
case p, =(0,100), p, =(0,-100), the initial configuration of the vehicle is 
q = ((—300,50),6,0), where there are eight cases of the initial 6 : 0, 45, 90, 135, 
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180, 225, 270, 315 degrees. Figure 5.8 shows the case where the initial 
configuration of the vehicle is g = ((—300,—50), 0,0). 
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Figure 5.7 : Simulation Result of Two Points, q = ((—300,50), 6,0) 
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Figure 5.8 : Simulation Result of Two Points, q = ((—300,-50),0,0) 


C. TWO LINES 

When we navigate the vehicle safely to make the same clearance from two 
lines, its trajectory becomes a line which is a bisector of two directed lines. So it is 
a Voronoi boundary of two lines. Assume a world consists of two directed lines q, 


and g,, there are two cases: they are parallel or not parallel (Figures 5.9 and 5.10). 


Interestingly, calculations for Ax, A@ and Ad are identical in each case. 
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SS 


q =((x,y), 0, K) 





Figure 5.10 : Not Parallel Directed Lines 


1. Evaluation of Ax 
When the vehicle's configuration is g=(p,6,k), 
Ak=Kk (5.41) 
Since final value of x on Voronoi boundary is zero. 
2. Evaluation of AO 


When the vehicle is on the Voronoi boundary its orientation is the 


average of 6, and 6,. Let this desired orientation be 6,, 
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6+ 0 


0,= normalize\( 52 - @, + 0, (5.42) 


Where normalizel is a function which normalizes its argument into a range of 


|-2.2| by addition of +nz if necessary. Then the variable A@ is evaluated as 


A@=6-86, (5.43) 
3. Evaluation of Ad 
Let d, be the signed distance from p to g,, and d, be the signed 


distance from p to q,. 


d, =~(x—x,)sin 0, + (y—y,)cos8, (5.44) 
d, =—(x —x,)sin 0, + (y— y, )cos 8, (5.45) 
The signed variable Ad is evaluated as 


We 28 (5.46) 


The signed distance from p to gq, is the distance between p and q,. If p 
is on the left of g,, then d,>0O and if p is on the right of g,, then d, <0 (Figure 
5.11). A similar argument holds for d,. 





Figure 5.11 : Signed Distance from p to q, 
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4. Simulation Res :it 

The result of algorithmic simulation can be found in Figures 5.12, 5.13, 
5.14, and 5.15. 

Figures 5.12 and 5.13 show the case where the lines are parallel. Figure 
5.12 shows the case gq, =((0,100),0,0), g, =((0,—100),0,0) and the initial 
configuration of the vehicle is g =((0,50),@,0), where there are eight cases of the 
initial 8 : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.13 shows the case 
where the initial configuration of the vehicle is g =((0,—5S0), 6,0). 

Figures 5.14 and 5.15 show the case where the lines are not parallel. 
Figure 5.14 shows the case g, =((0,0),90,0), g, =((0,0),0,0) and the initial 
configuration of the vehicle is gq =((50,150),0,0). Figure 5.15 shows the case 
where the initial configuration of the vehicle is q = ((150,50), 6,0). 
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Figure 5.12 : Simulation Result of Parallel lines, q = ((0,50),6,0) 
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Figure 5.13 : Simulation Result of Parallel Lines, q = ((0,-50),6,0) 
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Figure 5.14 : Simulation Result of Not Parallel Lines, q = ((50,150),6,0) 
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Figure 5.15 : Simulation Result of Not Parallel Lines, q = ((150,50),6,0) 


D. ONE POINT AND ONE LINE 

When we navigate the vehicle safely to make the same clearance from one 
point and one line, its trajectory becomes a parabola. So it is a Voronoi boundary 
of a point and a line. 


1. Definition of Parabola 


When a world consists of a point p, and a directed line q,, its Voronoi 
boundary becomes a parabola. A parabola is defined as the focus p, and the 
directrix q, : 

Dy =(Xp5Y,) (5.47) 
Io = (Xo» Yor 9p) (5.48) 
The directrix qg, has a direction, and hence, parabola has a direction. Let 


1 be the signed distance from p, to q. 


SN, 


l= —-(X%, —X,) sini, + (y= J) S Ose, (5.49) 
The signed distance from p, to q, is the distance between p, and q,. If p, is on 


the left of g,, then 1>0 andif p,is on the right of g,, then 1<0 (Figure 5.16). 





Figure 5.16 : Signed Distance from p, to q, 


a. Case 1>0 
Let 6, denote the orientation of the normal ray from p, to q,. We 


define a polar coordinate system whose pole is p, and whose initial ray is 86, 


(Figure 5.17). 


Py = (X,Y 


1>0 a 
"y | 


p=(r,9) 


= (X95 
Go = (Xo Yo» 9) 6, =0,-—a/2 





Figure 5.17 : Parabola (1 > 0) 
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6 — 0,77 2 (5.50) 
In this system, p is represented by (r,@), where r is the distance between p, and p 
and ¢ is the orientation from p, to p taken from the initial ray. In this case, we 
take ¢(-—2 < @ < x) counterclockwise from the initial ray. The coordinate of p in 
the global Cartesian system is 
(x,y) =(x, +rcos(O, + $),y, +rsin(O, + ¢)) 


-( + eC - sont 8) (5.51) 
1+cos@ 1+cos@ 


Let Y(p,,p) denote the orientation from p, to p. By definition, the angle a 


between Y(p,,p) and the orientation ‘Y of the tangent at p is defined as 


5 
a=s-5 (5.52) 


The orientation Y of the tangent at p in the polar coordinate system is 


¥=p+a=9+(F-£)-248 (5.53) 


The orientation @ of this tangent at p in the global coordinate system is 


9=0,+¥=(0,-=]+(Z42)-L46, (5.54) 
ND Dy 2 


Let s denote the arc length. Then the curvature x at p is 
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ae 
Be " 
“as = 
ay : (l+cos@) (+cosdy' 
(1+ cos)’ _(1+cos¢)” _ 
= = ] 
AL (1+cos@)’ oe ") 21/2 +2cos@ al + cos) 
=— #)} o 1 co51(9 
: Ti 2e0" (3) i. (3) (5.55) 


b. Case 1<0 


In this case (Figure 5.18), the orientation 9, of the initial ray in the 
global coordinate system is 


6=0.+n/2 (5.56) 


6.) 6,=9,+27/2 
=(XosYo> | 
Jo : 0» Yo» % p=(r,6') 


‘Y(D,;,P) 
er O 





= ie Pod 


Y 





Figure 5.18 : Parabola (1< 0) 


We take ¢ (-—2 < ¢@ < 2) clockwise from the initial ray 
g=-¢ a2!) 
Then the coordinate of p in the global Cartesian system is defined by Equation 
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(5.51), the orientation @ of the tangent at p in the global coordinate system is 
defined by Equation (5.55) and the curvature xk at p is defined by Equation 
(5.56). 
2. Evaluation of Ax 
When the vehicle's configuration is g=(p,@,K), assume the intersection 
of the orientation ‘¥(p,p,) and the parabola is ¢,.,4 =(Dpara>9 para» para) @S Shown in 
Figure 5.19. The variable Ax is evaluated as 
AK =K~K ja, (5.58) 


Note that Ax converges to zero as q approaches to q,,,,. 


q para = (2. ifs ? K nara ) 





Figure 5.19 : Evaluation of Ax 


6) /) 


3. Evaluation of A@ 
Let a be the difference between the vehicle's orientation @ and the 


orientation 6, of the initial ray. Also let B be the difference between the 


orientation ‘Y(p,p,) and the vehicle's orientation 6 (Figure 5.20). 





Then 
a= (p,p,)—- 6 (5.59) 
p=6-86, (5.60) 
When vehicle is on the parabola, 
a=B (5.61) 


Let this desired orientation be @,, 
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Y(p,p,)-@ 
6,= normalize PS. - | + 0, (5.62) 


Where normalizel is a function which normalizes its argument into a range of 


|-3.5 by addition of tnz if necessary. Then the variable A@ is evaluated as 
A@=0-86, (3.63) 


4. Evaluation of Ad 
Let d, be the distance between p and p,, and d, be the signed distance 


from p to g, (Figure 5.21). 


d, =4\(x-x,) +(y-y,)? (5.64) 


d, =-(x —x,)sin9, +(y— y,)cos 9, (5.65) 





Figure 5.21 : Evaluation of Ad 


The signed variable Ad is evaluated as 
Ad = d, -d, (if 1>0) (5.66) 
Ad =d, +d, (if 1<0) (5.67) 


abe 


5. Simulation Result 

The result of algorithmic simulation can be found in Figures 5.22, 5.23, 
5.24 and 5.25. Figures 5.22 and 5.23 show the case where the | Is positive. 
Figures 5.24 and 5.25 show the case where the | 1s negative. 

Figure 5.22 shows the case of p, =(0,200), q, =((0,0),0,0), the initial 
configuration of the vehicle is g = ((—300,200), 6,0), where there are eight cases of 
the initial @ : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.23 shows the 
case where the initial configuration of the vehicle is g =((—200,300), 6,0). 

Figure 5.24 shows the case of p, =(0,-200), g, =((0,0),0,0), the initial 
configuration of the vehicle is gq =((—300,—200),0,0), where there are eight cases 
of the initial @ : 0, 45, 90, 135, 180, 225, 270, 315 degrees. Figure 5.25 shows the 
case where the initial configuration of the vehicle is g = ((—200,—300), 0,0). 
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Figure 5.22 : Simulation Result of 1>0, q =((—300,200),0,0) 
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Figure 5.23 : Simulation Result of 1>0, q =((-—200,300),6,0) 
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Figure 5.24 : Simulation Result of 1<0, q =((-300,—200), 6,0) 
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Figure 5.25 : Simulation Result of 1<0, q =((—200,-300),6,0) 
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VI. CONCLUSION 


A. RESULTS 

Simulation results shows that when the objects are two points, two lines, or 
one point and one line, we can safely navigate the vehicle to achieve equal 
clearances from these objects. The motion of the vehicle is optimized at each 
point directly from the information of the obstacles near the vehicle. After 
calculating the steering function dx/ds which is the derivative of the curvature of 
the vehicle motion, the vehicle follows the Voronoi boundaries defined by the 
environment. 

Previous work in the motion control of the autonomous vehicle Yamabico 11 
used path tracking using a path specification for lines, circles and parabolas 
which are images of the path. Before the mission began the vehicle was given a 
track to follow; motion planning consisted of calculating the point on the track 
closest to the vehicle and then steering the vehicle to get onto the track. 

If the world navigated by the robot is large, it is complicated and inefficient to 
calculate every Voronoi boundary of this world. It is better to calculate the 
optimal motion of the vehicle at each point directly from the information of the 
world by computing the steering function dx/ds without calculating the track to 
follow. 

Unlike path tracking method, this method can be applied to avoid moving 
objects since we calculate the optimal motion of the vehicle at each point directly 
from the information of the world. Additionally, motion control is simpler and 


faster than in the path tracking method. 
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B. RECOMMENDATIONS 

Based on the results of this thesis, recommended follow on work includes 
leaving point calculation. 

There are two goals for planning autonomous vehicle navigation planning: 
shortest path and safe path. This safe navigation method is for only safe path 
planning. The short path planning is represented by path tracking using a path 
specification for lines, circles and parabolas which are images of the path. When 
we will combine this safe navigation method with short path planning, it will be 
necessary to calculate the leaving point from one path to another. Afterwards, the 


vehicle will continue on its way smoothly. 
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APPENDIX 


This appendix contains the C code for safe navigation which generated the 


results found in this thesis. 


A. POINTPATH.C 
FARA BIOGCIGCGIOCI I GGIOIO IIIA HI 1 


Author: Masahide Shirasaka 
Project: Yamabico Robot Control System 


Date: me 25 1994 
Revised July 12 1994 
File Nan:. pointpath.C 


Environment: GCC ANSI C compiler for the motorola 68020 processor 
Description: |§ This Program contains functions for safe navigation 


when the obstacles are two points. 
SR SE A | 


#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 
/{ =PI 

#define DPI 6.28318530717958647692 
// =2.0*PI 

#define HPI 1.57079632679489661923 
=F 0 

#define RAD 57.29577951308232087684 
// =180.0/PI 


* ° 
FILE *fp0, *fp1; 
eee SRL EE EAE Ee E EERE AS TOC EEE ES EEE ERE RES REE EE Ee 


Struct: POINT 


ee ee Teer EERE EER ERE EEE EEE ORE Eee ee eo / 
typedef struct { 

double x; 

double y; 


} 
POINT; 


en ae A ee ER TERRES EERE EERE ER EEE REET EH 


struct: CONFIGURATION 
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OR Re Ae ae ake ae Re Re fe ae Re he ate ie ae he ae ie ake 2c he ee fe Re ee fe oe Re ce ie 2c Ae ae ete eS AG A he A A 
typedef struct { 

POINT point; 

double theta; 

double kappa; 


CONFIGURATION; 


[RRRRE REAR RR EEE HE HRA KE ERE KE KKH A KR A A oe 


Function: —_normalize() 
Purpose: This procedure is for a function which normalizes an angle 
to within + or - PI values. 

BR 2 Fe He he She fe he he he he he eRe He he ae ee ee Ea a 
double normalize(double angle) 
{ 

angle = angle - DPI*(ceil((angle + PI)/DPI) - 1.0); 

return(angle); 


[RRR EERE EER KEE EE EREE ERE EEE RE EEE CEE EE EE EE EE EE ee 
Function: —normalizel() 
Purpose: This procedure 1s for a function which normalizes an angle 
to within + or - PI/2.0 values. 
Ste 5 fe Ke He She eRe he he he fe ee Se He fe he ie Si Se 2 ke Ae Ae ae Se Se 2k She Ee he he Se 2h he Ae ie Ae 2 2 he te Ae Ae tee oh eA 
double normalizel(double angle) 


{ 
while(angle > PI/2.0) 


angle = angle - PI; 


} 
while(angle <= -PI/2.0) 


angle = angle + PI; 


return(angle); 


} 


[RERRERERKELE REA ER EEE ERE EEE EERE KEKE EEL EE HA ET 


FUNCTION: InputPoints() 
Purpose: This procedure Inputs the configurations of two points. 
Me 2 2 2 ee te ae ae oe 2 ee RE EE EE EE A eh ee Fa 


void InputPoints(POINT &p1,POINT &p2) 


/* Point obstacle pl */ 

printf("Input Coordinates of the p1. \n"); 
printf("X=\n"),; 

scanf("%olf" ,&p1 .x); 
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printf(" Y=\n"); 
scanf("%lf" ,&pl.y); 


/* Point obstacle p2 */ 

printf("Input Coordinates of the p2. \n"); 
printf("X=\n"); 

scanf("%lf" ,&p2.x); 

printf("Y=\n") 

scanf("%1f" ,&p2.y); 


Sea ce ane re eerie ect eee re ye ete te AS RR Re A Re 6 Ae aS A Te te So a A a AE A 


FUNCTION: InputInitConfig() 
Purpose: This procedure Inputs the initial Configration of the vehicle, 
size constant and Step size. 
Bena te Ae Me A A ie te Ae A et a Ae ee Ae Re ee Ae eA ae Re Re ae eA a ee ee] 


void InputInitConfig(CONFIGURATION &q,double &s0,double &DS) 


/* Config of q */ 

printf("Input initial Configuration of the vehicle q. \n"); 
printf("X=\n"); 
scanf("Zlf",&q.point.x); 
printf("Y= \n"); 

scanf("Zlf" ,&q.point. y); 
printf("theta= \n"); 
scanf("%lf" ,&eq. theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa= \n"); 

scanf("Zlf" ,écq.kappa); 


/* Size constant */ 

printf("Input the size constant sO\n"); 
printf(sO =\n"); 

scanf("Zlf",&s0); 


[* DS */ 
printf("Input the step size DS.\n"); 
printf("DS =\n"); 


scanf("Zlf" ,&DS); 


Fea Megane tara Ne Re Aaa As re te fe te A Me Pee eA Te a a A oe Pe AE He ee A A oe 
FUNCTION: GetlInitThetaDesire() 
Purpose: This procedure is for a function which compute the value of 


desired initial theta. 
FEE IG RGEC GG ka aE ack 4k / 


double GetInitThetaDesire(CONFIGURATION q,POINT p1,POINT p2) 
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POINT po; 


pO.x = (p1.x + p2.x)/2.0; 
p0.y = (pl.y + p2.y)/2.0; . 
return(normalizel ((atan2(p1.y-q.point.y,p1.x-q.point.x) 
+ atan2(p2.y-q.point.y,p2.x-q.point.x ))/2.0 
- atan2(p0.y-q.point.y,p0.x-q.point.x) ) 
+ atan2(p0.y-q.point.y,pO.x-q.point.x)); 
[PRR RR A HR RR AR AK RR A AE FE A AR AR AS Se AR 1 AR A RS AG ie I 
FUNCTION: GetConstants() 


Purpose: This procedure is for a function which compute the value of 


constants k, a, b, and c. 
st Se oe oe ie fe fe Se se se oe ie ie he a pea Fe he He he a te Re Ae Ae a ie ee Ae Ae ee le tee 


void GetConstants(double SO,double &a,double &b,double &c) 
{ 
double ConstK; 


ConstK=1.0/SO; 
a=3.0*ConstK; 
b=3.0*ConstK *ConstK; 
c=ConstK*ConstK*ConstK; 


} 


[RR HE EA EE EE ER A ee a Er ta 
FUNCTION: GetSteeringFunc() 
Purpose: This procedure 1s for a function which compute the value of 


steering function dk/ds. 
Bt Re Ae he Se 9 Ae ee ee 2 2 Ae ee Ae ae ee Re ee 2 ie oe Ae a AeA I Ee A ra 


void GetSteering(double a,double b,double c, CONFIGURATION gq, 
POINT p1,POINT p2,double &thetaDesire,double &u) 
{ 


double deltaKappa,deltaTheta,deltaDist,d1,d2; 


/* Calculate deltaKappa */ 
deltaKappa = q.kappa; 


/* Calculate deltaTheta */ 

thetaDesire = normalize 1 ((atan2(p1 -y-q.point. +p X-q.point.x) 
+ atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 
- thetaDesire) + thetaDesire; 

deltaTheta = normalize(q.theta - thetaDesire): 


/* Calculate deltaDist */ 
dl = sqrt((p1.x-q.point.x)*(p1.x-q.point.x) 
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+ (pl.y-q.point.y)*(p1.y-q.point.y)); 
d2 = sqrt((p2.x-q.point.x)*(p2.x-q.point.x) 
+ (p2.y-q.point.y)*(p2.y-q.point.y)); 
if (atan2(p1.y-q.point.y,p1.x-q.point.x) 
- atan2(p2.y-q.point.y,p2.x-q.point.x)>0) 
deltaDist = d2 - d1; 
else 
deltaDist = d1 - d2; 


/* Calculate Steering fucnction = u */ 
u = -(a*deltaKappa + b*deltaTheta + c*deltaDist); 


ee te ee ee Ee EERE EEE EEE SEE EE EEE SEEDS ES Ee REE 


FUNCTION: GetDkappa() 
Purpose: This procedure is for a function which computes the value of 
dKappa. 


ee See ee eee Eee eee ERE EE EEE EER ERE RCE CERES | 


double GetDkappa(double u,double ds) 


return(u*ds); 


ec Te eee CEES TEES ESEREE EEL EEE EL EERE EELS EER EH EE 


FUNCTION: GetKappa() 
Purpose: This procedure is for a function which computes the value of 
Kappa. 


ee a ee eee ERE eT ELS EE EEE EEE EERE EEE EEE ERS ES / 


void GetKappa(double dkappa, CONFIGURATION &q) 


q.kappa=q.kappa + dkappa; 
} 


a ee ee EEE EEE EEE EERE E EEE ERE EE ERE EEE EEE ESE EE EE He 


FUNCTION: GetDthetaQ) 
Purpose: This procedure is for a function which computes the value of 


dtheta. 
er ee ee ree ee eee SES EES Ee ES CREE ES LEE EEE EERE EEE EE SX / 


double GetDtheta(> CONFIGURATION ql1,double ds, 
return(q1.kappa * ds); 
} 


er Cee Eee ee EEE EE EES EE EE ee ee a Ee EE 


FUNCTION: next() 
Purpose: This procedure is for a function which computes the next 
configration of the vehicle. 
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oe AR oe a ae ok ee ee EE EE RES SS ee Fe eee 


void next(double ds,double dtheta,double &s, CONFIGURATION &q) 
{ 


CONFIGURATION ql; 

/* CONFIGURATION of q1 */ 

qi.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta*dtheta/24.0)*dtheta*ds; 
ql.theta = dtheta; 


s=s+ds; 
/* CONFIGURATION of q */ 
q.point.x = q.point.x + ql1.point.x*cos(q.theta) - q1.point.y*sin(q.theta); 
q.point.y = q.point.y + q1.point.x*sin(q.theta) + q1.point.y*cos(q.theta); 
q.theta = q.theta + q1.theta; 

j 


[FEE EEE EEE EEE EEE LEEKS EE REE EEE EEE EE EEL EE EEE EE A 


FUNCTION: Openfile() 


Purpose: This procedure opens the output file. 
He Se he He ee he he he he ee 2 2 2 he he he he fe ae ae He 2 ee ee A He Hee AE Ae Ae ie he Be Ae AAG Ae Oe A A 2A 


void Openfile(>CONFIGURATION gq,double s) 
{ 


fpO = fopen("path.dat","w"); 
fpl = — fopen("path","w *f w"): 


fprintf(fp0,” s x y  theta[deg) kappa "); 
fprintf(fp0," u ‘ae deltaTheta deltaDist\n"); 
printf(" s X theta[deg] kappa\n"); 


fprintf(fp0,"%4.4f God, 4f %4.4f %4.4f %4.4fn",s, q.point.x, q.point.y, 
q.theta*RAD,q.kapp a); 
printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", s, q.point.x, q.point.y, 
q.theta*RAD,q.kappa); 
7 fprintf(fp1,"%f Yf\n", q.point.x, q.point.y); 


[FRRREKE LEAKE EAELAEE EERE EERE EEREREEAEEKE SLADE Fe 


FUNCTION: PrintfileQ 
Purpose: This procedure prints the result to the file. 
a He Me 9 he He ee ie He He He ae She Ee oe ie Ee he 2 ie Ae he 2 ae oe Ee te ae oe Ee oe eo ak A ee Oh eh 


void PrintfileeCONFIGURATION q,double s) 


{ 
fprintf(fp0,"%4.4f = Af %4.4f %4.4f %4.4f ", 
S, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
printf("%4.4f %4.4f %4.4f %oA.AF %d. 4f\n", 
S, q.point.x, q.point.y, q. ‘theta*RAD,q.kappa); 
fprintf(fp1,"%f %E\n", q.point.x, q.point.y); /* for gnuplot */ 
} 
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a oe i hee EEE EEE EE EES EEE EEE ERE TREE ES 


FUNCTION: mainQ 


ener ene eo ana Ae Oe Ra ee he eo aS a he ee ee ee of 
void main(void) 


CONFIGURATION gq; 

POINT pl,p2; 

double u; /* steering function */ 
double DS,s,s0,a,b,c,thetaDesire; 
double dkappa,dtheta; 


InputPoints(p1 ,p2); 
InputInitConfig(q,s0,DS); 
GetConstants(s0,a,b,c); 


thetaDesire=GetInitThetaDesire(q,p1,p2); 


Sa 00): 
Openfile(q,s); 


do 


GetSteering(a,b,c,q,p1,p2,thetaDesire,u); 
dkappa = GetDkappa(u,DS); 

GetK appa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 

Printfile(q,s); 


while(s<=800.0); 


fclose(fp0); 
fclose(fp1); 


. LINEPATH.C 


Deemed hecho FX RUREE he ee ee A a ee OE He HK a a a Oe a 


Author: Masahide Shirasaka 

Project: Yamabico Robot Control System 

Date: June 26 1994 

Revised: July 12 1994 

File Name: linepath.C 

Environment: GCC ANSI C compiler for the motorola 68020 processor 
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Description: This Program contains functions for safe navigation 


when the obstacles are two directed lines. 
BIO 2 Re ee Ae ee A AS A eRe ee AE AE AE AG A SS RO oR PS A A Ee 


#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 
i =FPi 

#define DPI 6.28318530717958647692 
// v= 2-02PI 

#define HPI 1.57079632679489661923 
ii =Pi/20 

#define RAD 57.29577951308232087684 
// =180.0/PI 


FILE *fp0, *fp1; 
[FEAR FE ee 2 ie ae 2 Ae ae ee 2 ke he Ee ee ee ee oe ie ee Ee EE A A ee 


struct: POINT 


ae oe he He hehe he 2 he he he fe ee 2 he ee 2 a ee ee a Se ee he 2 2 Re fe fee ee he Ae ee ee 


typedef struct { 
double x; 
double y; 


POINT; 
[HER EEER EER RARE EER ERR EK ER KEE EEE AEE EE ER ER ee 


struct: CONFIGURATION 


Be He He Re fe ie he 2 oe ie he he ee ke he ake Fe Ae a a ae Fe Ae ee Fe Fe Ae ae 2 Re Fe he 2 Fe Hee A AS A Re Pe AG tetra 


typedef struct { 
POINT point; 
double theta; 
double kappa; 


} 
CONFIGURATION; 


[ERE EERE ERE RARE ERE KRERAEREAER EERE AE EEK KE ae 


Function: _—_ normalize(angle) 
Purpose: This procedure is for a function which normalizes an angle 


to within + or - PI values. 
BR 3 Hee He He He Ree Ae He 26 fe 2 he he he fe Se ie ie he ae fe Se Me Ne le ee ee ee ee 


double normalize(double angle) 


angle = angle - DPI*(ceil((angle + PI)/DPI) - 1.0); 
return(angle); 


a) 


Datars Seleles es tac toh ete te Pee Re oP he oe ee ee eee ce ah ke oe kok oh ee hoe kok ee 
Function: — normalizel (angle) 
Purpose: This procedure is for a function which normalizes an angle 
to within + or - PI/2.0 values. 
Bedarra ee eee tenance Ae Oe ke AS OR eR i Sh ee Ae ak Chee ee ee ee / 


double normalizel (double angle) 
while(angle > PI/2.0) 
angle = angle - PI; 
while(angle <= -PI/2.0) 
angle = angle + PI; 


return(angle); 


ee ee ee ee EE ee eh 


FUNCTION: InputLines() 
Purpose: This procedure Inputs the configurations of two Lines. 
Brame Ste Na ot a er ete Sen i te ee fee ee te te ei a ee ef 


void InputLines(CONFIGURATION &q1,CONFIGURATION &q2) 
{ 


/* Line obstacle ql */ 

printf("Input initial Configuration of ql. \n"); 
printf("X=\n"); 

scanf("%lf",&q1 .point.x); 

printf("Y= \n"); 

scanf("%lf" ,&q1.point.y); 

printf("theta= \n"); 

scanf("%lf" ,&q1.theta); 
q1.theta=normalize(q1.theta/RAD); 

ql .kappa=0.0; 


/* Line obstacle q2 */ 

printf(“Input initial Configuration of q2. \n"); 
printf(""X=\n"); 

scanf("[%olf" ,&q2.point.x); 

printf("Y=\n"); 

scanf("Zlf" ,&q2.point.y); 

printf("theta= \n"); 

scanf("%lf" ,&q2.theta); 
q2.theta=normalize(q2.theta/RAD); 
q2.kappa=0.0; 
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[ER RERERE ER AEE REE RAE KE EH KH A A ERE ee a Ee 


FUNCTION: InputInitConfig() 
Purpose: This procedure Inputs the initial Configration of the vehicle, 
size constant and step size. 


Pe ee fe fe ee ke ake ke he 2k ake 2 2 2 2 ae 2 Ie aS fe Ae ie ie ke fe he eS SAG AS Ae fe Ac AS AG ks A AE A A A 


void InputInitConfig(CONFIGURATION &q,double &s0O,double &DS) 


} 


/* Config of q */ 

printf("Input initial Configuration of the vehicle q. \n"); 
printf("X=\n"); 

scanf("%lf" ,é&¢q.point.x); 
printf("Y=\n"); 
scanf("%lf",&q.point y); 
printf("theta= \n"); 
scanf(" olf" ,&q.theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa= \n"); 

scanf("Z%lf" ,&q.kappa); 


/* Size constant */ 

printf("Input the size constant sO\n"); 
printf(""sO =\n"); 

scanf("Zlf" ,&s0); 


/* DS */ 
printf("Input the step size DS.\n"); 
printf("DS =\n"); 


scanf("%lf" ,&DS); 


[FR EA A A hee ee He he ee ee ee Me Be ae He Be Me fe 2 he Me ie he oe oe fe ae oe ie Me ae oe Ae es 2 ic Neale a 2% era 


FUNCTION: GetConstants() 
Purpose: This procedure is for a function which computes the value of 
constants k, a, b, and c. 


3 2 He 9 ie Re ee Re ie ee oe ee he ie Re Ie 2 2 6 Ae Ae 2 Fe fe As ee ee 2 Fe Ae eS Fe eS AS oe ooh 


void GetConstants(double SO,double &a,double &b,double &c) 
{ 


} 


double ConstK; 


ConstK=1.0/SO; 
a=3.0*ConstK; 
b=3.0*ConstK*ConstK; 
c=ConstK*ConstK*ConstK; 


[PRE AER EE EE EE RE EE Me Ae OE eee 


FUNCTION: GetSteeringFunc() 
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Purpose: This procedure is for a function which computes the value of 


Steering function dk/ds. 
Beery cyte en ry a ere Oe ae Pee A Ee Pe a ce eA A A oi ee A Ce a tO a ae ee a / 


double GetSteering(double a,double b,double c, CONFIGURATION gq, 
CONFIGURATION q1,CONFIGURATION q2) 


double deltaKappa,deltaTheta,deltaDist,thetaDesire,d1,d2; 


/* Calculate DeltaKappa */ 
deltaKappa = q.kappa; 


/* Calculate DeltaTheta */ 
thetaDesire = normalizel ((q1.theta+q2.theta)/2.0 - ql.theta) + q1.theta; 
deltaTheta = normalize(q.theta - thetaDesire); 


/* Calculate DeltaDist */ 
dl = -(q.point.x - q1.point.x)*sin(q1.theta) 

+ (q.point.y - ql.point.y)*cos(q1 .theta); 
d2 = -(q.point.x - q2.point.x)*sin(q2.theta) 

+ (q.point.y - q2.point.y)*cos(q2.theta); 
deltaDist = (d1 + d2)/2.0; 


/* Calculate Steering fucnction = u */ 
retum(-(a*deltaKappa + b*deltaTheta + c*deltaDist)); 


Samia eer te TRG Re ECF 6 ASG oe OAS le ee AR ae 20 hee aR i fe He oe ie a oe oe ae ie ie a ee 


FUNCTION: GetDkappa() 
Purpose: This procedure is for a function which computes the value of 
dKappa. 


Seine 2 Nee Pee eee ae oe A A A I oh fo IR Ra ee ee ae ae Ok ek ee ef 


double GetDkappa(double u,double ds) 
return(u*ds); 


ene ee ea ee ee a eee Rk ee ok a ee Oe ee on 


FUNCTION: GetKappa() 
Purpose: This procedure is for a function which computes the value of 
Ka 


ern tet tere oh mate ee are we chet, ee ee ee ee OR ee OR RE Ee | 


void GetKappa(double dkappa, CONFIGURATION &q) 
{ 


q.kappa=q.kappa + dkappa; 
} 


et ee et ee ee eee ee ee ee ee ee ee ee ee 


Pe 


FUNCTION: GetDtheta() 
Purpose: This procedure is for a function which computes the value of 
dtheta. 
Be A Se 5 oe 2 2 fe he fee 2 Se ee ee ee Ae Ae Ae ee ake Re i ie ie Ae AS Ae Ae AG ee iit 2 A AS Ae ce 


double GetDtheta(; CONFIGURATION ql,double ds) 


return(q1.kappa * ds); 


[FER EE A A AE EE EE Re AR I GR A eee 


FUNCTION: next() 
Purpose: This procedure is for a function which computes the next 
configration of the vehicle. 
oe she Se fe se She oie He he He fe he ee Oe 2 ee he he Ne ke Ae eile ek ie ie ee a a RS ah oh ta a A A 


void next(double ds,double dtheta,double &s,; CONFIGURATION &q) 


{ 
CONFIGURATION a]: 


/* CONFIGURATION of q1 */ 

ql.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta*dtheta/24.0)*dtheta*ds; 
ql.theta = dtheta; 


S=s+ds; 


/* CONFIGURATION of q */ 
q.point.x = q.point.x + ql.point.x*cos(q.theta) - q1.point.y*sin(q.theta); 
q.point.y = q.point.y + ql.point.x*sin(q.theta) + ql .point.y*cos(q.theta); 
q.theta = q.theta + q1.theta; 

} 


[RE EH eH HH He HK EHH He He He HH He He Hee Hee EE 


FUNCTION: Openfile() 
Purpose: This procedure opens the output file. 
BR Re He He fe Ae ae he he he ae ee ie ee a ee He ee AG ee ee he he ae ee a a i Re 


void OpenfileeC CONFIGURATION gq,double s) 


fpO = fopen("path.dat","w"); 

fpl = = fopen("path", "w"): 

fprintf(fp0," s x y  theta[deg] kappa "); 
fprintf(fp0, " U See deltaTheta deltaDist\n"); 


printf(" s x theta[deg] kappa\n"); 
fprintf(fp0,"%4.4f Soh, 4f %4.4f %4.4f %4.4f\n",s, q.point.x, q.point.y, 
q.theta*RAD,q.kappa); 
printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", s, q.point.x, q.point.y, 
q.theta*RAD,q.kappa); 


fprintf(fp1,"%f %f\n", q.point.x, q.point.y); 
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} 


ee ee EE eee eR EEE EES EER eX 


FUNCTION: Printfile() 


Purpose: This procedure prints the result to the file. 
SSS ee ee eee ee ee ee ee | 


void Printfilee CONFIGURATION q,double s) 


{ 
fprintf(fp0, Ripe Af 24.4f 244f 244f 04.4f ", 
S, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
printf("%4.4f A Af %4.4f %4.4f %4. 4f\n", 
S, q. point. X, q.point.y, q. theta*RAD,q. kappa); 
fprintf(fp1,' ‘of Sof\n" , q.point.x, q.point.y); /* for gnuplot */ 


oe ee CT ee a a 


FUNCTION: main 


Sige te dy nt eet ee en ee ee on aden ee 
void main(void) 


CONFIGURATION gq,q1,q2; 
double u; /* steering function */ 
double DS,s,s0,a,b,c; 

double dkappa,dtheta; 


InputLines(q1,q2); 


InputInitConfig(q,sO,DS); 
GetConstants(s0O,a,b,c); 


s = 0.0; 

Openfile(q,s); 

do 

u = GetSteering(a,b,c,q,q1,q2); 
dkappa = GetDkappa(u,DS); 
GetK appa(dkappa,q); 
dtheta=GetDtheta(q,DS); 


next(DS,dtheta,s,q); 
Printfile(q,s); 


while(s<=800.0); 


fclose(fp0); 
fclose(fp1); 
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C. PARAPATH.C 


[REERR AREER RE ARH EEE EAE EK EE AKER RH ER RH Fs A lets 


Author: Masahide Shirasaka 

Project: Yamabico Robot Control System 

Date: May 15 1994 

Revised: June 17 1994 

File Name: parapath.C 

Environment: GCC ANSI C compiler for the motorola 68020 processor 
Description: This Program contains functions for safe navigation 


when the obstacles are one point and one directed line. 
a ee he fe ee fe ae 2 2 ee ae oe ee 2 oe ie EE a OR Ae AAS oA EAS 2 eA A A A A A ee 


#include <stdio.h> 

#include <math.h> 

#define DR (PI/180.0) 

#define PI 3.14159265358979323846 


[ 
#define DPI 6.28318530717958647692 
/ =2.0*PI 
#define HPI = 1.57079632679489661923 
// =PI/2.0 
#define RAD 57.29577951308232087684 
// =180.0/PI 


FILE *fp0, *fp1, *fp2, *fp3; 


[RRRERRKER RARER EERE ER AR EERE EHH RRS EE ee 


struct: POINT 


ate He fe ee fe Se He fe ee He ee he ee He ee He He He ee He ee he ee ie hee Se Ae he ee Ee Oe es 


typedef struct { 
double x; 
double y; 


POINT; 


[RRR E ERE EERE RE EEE ER EE EE EEE EE 


struct: CONFIGURATION 


ae he Ae ae 2 he oe oe 2 ie he ae ie ae 2 ie ee a ae 2 Oe he He ae 2 A ae a eA 


typedef struct { 
POINT point; 
double theta; 
double kappa; 


CONFIGURATION; 
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TEE EEL SIGE Ea Sa SE SE a SA SE SS Se SSL al ie a Si St Seach 


Function: — normalize() 
Purpose: This procedure is for a function which normalizes an angle 
to within + or - PI values. 
Bae eee ee ee eee EES EES EEE ERE EES te eee / 


double normalize(double angle) 


angle = angle - DPI*(ceil((angle + PI)/DPI) - 1.0); 
return(angle); 


[RA A A A RR CC a Ci ie keke keke ae keke dea ako keke ok ok oo oko oe oe ok ok ae ak a ke ak ak 


Function: —normalizel() 
Purpose: This procedure is for a function which normalizes an angle 
to within + or - PI/2.0 values. 
oe eee ee ee EEK ER EERE REE EEE RES EES ESE EE EES 


double normalize1 (double angle) 


{ 
oO > PI/2.0) 


angle = angle - PI; 

} 

while(angle <= -PI/2.0) 
{ 

angle = angle + PI; 


return(angle); 


ieee © ene eee Ee EES EES E ERE eee Eee Eee 


FUNCTION: InputParabola() 
Purpose: This procedure Inputs the Configrations of one point and 
one directed line. 


eg eee eee ee eee EEE E ELE ER SS CEE EE Ee | 


void InputParabola;> CONFIGURATION &q0,POINT &p) 
{ 


is Config of gO */ 

printf("Input initial Configuration of the qO (directrix). \n"); 
printf("X=\n"); 

scanf("Zlf", &q0. point.x); 

printf("Y=\n 2); 

scanf("Z%lf" ,&q0.point.y); 

printf("theta= \n"); 

scanf("%lf" ,&q0.theta); 
qO.theta=normalize(q0.theta/RAD); 

qO.kappa=0.0; 


Se 


[FR ARR A ie A Ae ee Ae Oe AE A oe AS RG RE AG ee A he AS oe Ae AS oe he 2 ke 2 se ot i hie ea 


} 


[REPRE RARER EK EEE ERE EEK AEE KALE RE EAE HF EEF CE ET DT 


OR ie Re Re ie he He He He ee Se ee 2 24th Fe fe fe he ae 2s ae ke 2h ie he Ae he ele ae ae ois Soe iS A es I ee 


double GetSize(CONFIGURATION q0,POINT p) 


/* Point obstacle */ 


printf("Input Coordinates of the pf (focus). \n"); 


printf("X=\n"); 
scanf(" Zlf" ,&p.x); 
printf" Y= \n"); 
scanf("%lf" ,&p.y); 


FUNCTION: InputInitConfig() 


Purpose: This procedure Inputs the initial Configration of the vehicle, 


size constant and step size. 
Ae Ae Ae Ae Ae He He Ae Ae Ae ee ee He He ee ee ee ee ee ee ee He ee ee ee ee KK KK KK Ke KK HK KK 


void InputinitConfig(CONFIGURATION &q,double &s0,double &DS) 


/* Config of q */ 


printf("Input initial Configuration of the vehicle q. \n"); 


printf("X= \n"); 
scanf("Zlf" ,&q.point.x); 
printf("Y=\n' '); 

scanf("%lf" ,&q.point.y); 
printf("theta= \n"); 

scanf("%lf" ,&¢q.theta); 
q.theta=normalize(q.theta/RAD); 
printf("kappa= \n"); 

scanf("%lf" ,&q.kappa); 


/* Size constant*/ 

printf("Input the size constant sO\n"); 
printf("sO =\n"); 

scanf("Zlf" ,&s0); 


[* DS */ 

printf("Input the step size DS.\n"); 
printf("DS =\n"); 

scanf("%lf" ,&DS); 


FUNCTION: GetSize() 


Purpose: This procedure is for a function which computes the value of 


size of the parabola. 


return(-(p.x-qO.point.x)*sin(qO.theta) + (p.y-q0.point.y)*cos(q0.theta)); 
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(nee eee ee ee eee er EET ee EEE ee eee eX 


FUNCTION: GetConstants() 
Purpose: This procedure is for a function which computes the value of 
constants k, a, b, and c. 
aa rag ea ee get tg ete etary Ae ee eas tet A re a a a ee ef 


void GetConstants(double SO,double &a,double &b,double &c) 
{ 
double ConstK; 


ConstK=1.0/SO; 
a=3.0*ConstK; 
b=3.0*ConstK *ConstK; 
c=ConstK *ConstK*ConstK; 


} 


ee ee ee EE ES ee et ee EEE Se ee Eee et 


FUNCTION: GetSteeringFunc() 
Purpose: This procedure is for a function which computes the value of 
steering function dk/ds. 
ee ee ee ee ee eer Eee EEE EE ER EEE RRS Re 
double GetSteering(double a,double b,double c, CONFIGURATION gq, 
CONFIGURATION qO,POINT p,double size) 


double kappaPara,phi,deltaKappa,thetaN ,thetaDesire, 
deltaTheta,deltaDist,d1 ,d2; 


/* Calculate DeltaKappa */ 
if ( size >= 0.0 ) 
phi = 
normalize(atan2(q.point.y-p.y, q.point.x-p.x) - (qQ.theta-PI/2.0)); 
else 
phi = normalize(-atan2(q.point.y-p.y, q.point.x-p.x) + 
(qO.theta+PI/2.0)); 
kappaPara = cos(phi/2.0)*cos(phi/2. 0)*cos(phi/2.0)/size; 
deltaKappa = q.kappa - kappaPara; 


/* Calculate DeltaTheta */ 

thetaN=((size >= 0.0) ? (qO.theta - PI/2.0):(qO.theta + PI/2.0)); 
thetaDesire = 

normalize] ((atan2(p.y-q.point.y, p.x-q.point.x) + thetaN)/2.0 - qQ.theta) 
+ qQ.theta; 

deltaTheta = normalize(q.theta - thetaDesire); 


/* Calculate DeltaDist */ 


d1 = sqrt((p.x-q.point.x)*(p.x-q.point.x) + (p.y-q.point.y)*(p.y-q.point.y)); 
d2 = -(q.point.x-q0.point.x)*sin(qQ.theta) 
+ (q.point.y-qO.point.y)*cos(q0.theta); 
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if (size >= 0.0) 

deltaDist = d2 - dl; 
else 

deltaDist = d2 + dl; 


/* Calculate Steering fucnction = u */ 
return(-(a*deltaKappa + b*deltaTheta + c*deltaDist)); 
} 


[RERRERRERREE EEE EER EREA ERE EEE EEK EE AE EE eee 


FUNCTION: GetDkappa() 
Purpose: This procedure is for a function which computes the value of 
dKappa. 


Me oe 2 ee He he 2 He He Me He ee ee He He ee eA eG AE A oe AS Oe ee 


double GetDkappa(double u,double ds) 


return(u*ds); 


[RERRREKEEEEAREEEEEREREES EEE REESE FE REE EEE ES EEE eee 


FUNCTION: GetKappa() 
Purpose: This procedure is for a function which computes the value of 
Ka 


oe 2 He fe ae Fe he Re ee 2 Se he 2 ie He 3 3 He ae ae He Ae ee HG ae Oe Ae He 2 Oe As 2 he He Ae a Re HE ee Fe AG a atc Ae 2 A aera 


void GetKappa(double dkappa, CONFIGURATION &q) 
{ 


q.kappa=q.kappa + dkappa; 
} 


[6 Ee He 9 Ee eH a a 9 A A Ee a a a a 2 A He a ee Ae ee te ct A A 


FUNCTION: GetDtheta() 
Purpose: This procedure is for a function which computes the value of 


dtheta. 
a 2 He se 5c fe fe fe Re he aie fe fe he Ee he He Ac He he te ic le ie Ae Ae 9 Ee Hee te ee he Ec He he Ae He oe oe 2 Ae che ie 


double GetDtheta(; CONFIGURATION ql,double ds) 


{ 
returmn(ql.kappa * ds); 
} 


[78 A Fe He EE A EE EE EE ER EE EE ee 


FUNCTION: next() 
Purpose: This procedure is for a function which computes the next 
configration of the vehicle. 
FE GS GG SGC I GGG GIGI III II IK / 


void next(double ds,double dtheta,double &s, CONFIGURATION &q) 
{ 
CONFIGURATION ql; 


62 


/* CONFIGURATION of q1 */ 
qi.point.x = (1.0 - dtheta*dtheta/6.0)*ds; 
ql.point.y = (0.5 - dtheta*dtheta/24.0)*dtheta*ds; 
qi.theta = dtheta; 


s=s+ds; 

/* CONFIGURATION of gq */ 

q.point.x = q.point.x + ql.point.x*cos(q.theta) - q1.point.y*sin(q.theta); 
q.point.y = q.point.y + ql.point.x*sin(q.theta) + q1.point.y*cos(q.theta); 
q.theta = q.theta + q1.theta; 

} 


TESS SE AD SE ET EE a a a ia alas 


FUNCTION: Openfile() 
Purpose: This procedure opens the output file. 
ee a ee eh ee eRe ETE EEE ERE | 


void OpenfileeCONFIGURATION q,double s) 
{ 


fpO = fopen("path.dat","w"); 

fp1 = fopen("path","w w"); 

fprintf(fp0,”" s x y theta[deg) kappa “); 

fprintf(fp0," u deltaKappa deltaTheta deltaDist\n"); 

printf(" s y _ theta{deg] kappa\n"); 

fprintf(fp0, "God. 4f %4.4f %4.4f %4.4f %4.4f\n",s, q.point.x, q.point.y, 
q.theta*RAD,q.kappa); 

printf("%4.4f %4.4f %4.4f %4.4f %4.4f\n", s, q.point.x, q.point.y, 
q.theta*RAD,q.kappa); 

fprintf(fp1,"%f Zof\n", q.point.x, q.point.y); 

} 


3 2 2K 2 2 oe 2 oe 2 oe 2 ae 2 ak ie 2 2 2 ae ie 2 ee 2 2 2 ok 2 oe 2 oe a 2 aK 2 a a oe eo EO oe 2 KK OK KK OK 


FUNCTION: PmnintfileQ 
Purpose: This procedure prints the result to the file. 
aaa aS oe ae ea ote 2 ee Ye oe tar eee A a a OR RE | 


void Printfile(CONFIGURATION q,double s) 


{ 
fprintf(fp0, ~ 4f HAAF HMA 4f M4 4f H4.4f ", 
S, q.point.x, q.point.y, q.theta*RAD,q.kappa); 
printf("%4.4f 4. 4f %4.4f %4.4F %d. 4f\n", 
S, q. point. X, q.point.y, q. theta*RAD,q. kappa); 
fpnntf(fp1, "of Jof\n"” ,q.point.x, q.point.y); /* for gnuplot */ 


ee oa ee a ee EEE EE EET EEE ESTERS TEES EE EK Eee 


FUNCTION: mainQ) 


ee ee eT EET EEE EES EERE ER ES EEK) 
void main(void) 
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CONFIGURATION q0,q; 
POINT p; 

double u; /* steering function */ 
double DS,s,s0,a,b,c,size; 

double dkappa,dtheta; 


InputParabola(q0,p); 
size=GetSize(q0,p); 


InputInitConfig(q,s0,DS); 
GetConstants(sO,a,b,c); 


s= 0.0; 
Openfile(q,s); 


do 


{ 

u = GetSteering(a,b,c,q,q0,p,size); 
dkappa = GetDkappa(u,DS); 
GetK appa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 

Printfile(q,s), 


while(s<=2000.0); 


fclose(fp0); 
fclose(fp1); 
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